import numpy as np

INF = 1000000
NODE_SIZE = 6
dis = np.array([[0, 2, 3, INF, 5, INF],
                [2, 0, INF, 4, INF, INF],
                [3, INF, 0, INF, 4, INF],
                [INF, 4, INF, 0, 1, 2],
                [5, INF, 4, 1, 0, 5],
                [INF, INF, INF, 2, 5, 0]])
h = np.array([INF, 2, 5, 2, 1, 0])
g = np.array([INF] * NODE_SIZE)
# f=h+g
close_list = close_set = []
open_list = fringe = []
route = np.array([0] * NODE_SIZE)
name = ["Start", "A", "B", "C", "D", "Goal"]
start = 0
goal = NODE_SIZE - 1


def name_of(node):
	return name[node]


def print_route(now):
	if (now == start):
		print("%s->" % name_of(now), end='')
		return
	print_route(route[now])
	if (now == goal):
		print("%s . The cost is %d" % (name_of(now), g[goal]))
	else:
		print("%s->" % name_of(now), end='')


def best_in_the_open_list():
	minn = INF
	min_i = 0
	for i in open_list:
		if (g[i] + h[i] < minn):
			minn = g[i] + h[i]
			min_i = i
	return min_i


def AstarSearch():
	g[start] = 0
	open_list.append(start)
	while (len(open_list) != 0):
		now = best_in_the_open_list()
		for i in range(0, NODE_SIZE, 1):
			if (i == now or dis[now][i] == INF or i in close_list):
				continue
			if (g[i] > g[now] + dis[i][now]):
				if (i not in open_list):
					open_list.append(i)
				route[i] = now
				g[i] = g[now] + dis[i][now]
		open_list.remove(now)
		if (now == goal):
			print_route(now)
			return True
		close_list.append(now)
	print("No solution found!")
	return False


if __name__ == '__main__':
	AstarSearch()
